#include <iostream>

#include "motor_ctrl.h"

using namespace std;

Motor_Ctrl* g_motor_ctrl;

/*  */
Motor_Ctrl::Motor_Ctrl(Can_Ctrl *p_can_ctrl, uint8_t num,  uint8_t id_array[], Motor_Type type_array[]) 
{
    cout<<"Motor_Ctrl"<<endl;
    this->p_can_ctrl = p_can_ctrl;

    num = ((num > MOTOR_NUM_MAX) ? MOTOR_NUM_MAX : num);
    this->num = num;
    for(uint8_t i = 0; i < num; i++) {
        if(MOTOR_TYPE_AK == type_array[i]) {
            p_motor[i] = new Motor_AK(p_can_ctrl, id_array[i], type_array[i]);
        } else {
            p_motor[i] = new Motor_YBT(p_can_ctrl, id_array[i], type_array[i]);
        }
    }
}

Motor_Ctrl::~Motor_Ctrl() 
{
    cout<<"~Motor_Ctrl"<<endl;
    for(uint8_t i = 0; i < num; i++) {
        delete p_motor[i];
    }
}

int Motor_Ctrl::motor_func(uint8_t num, Motor_Mode mode, double value[], uint8_t len)
{
    int32_t data_len;
    uint8_t dataframe[8];
    uint8_t mo = p_motor[num]->get_motor_mode_map(mode);

    //CAN没准备好
    if(0 == p_can_ctrl->is_ready()) {
        return -1;
    }

    CAN_RxHeaderTypeDef header;
    header.RTR = CAN_RTR_DATA;
    if(p_motor[num]->get_motor_type() == MOTOR_TYPE_AK) {
        header.ExtId = p_motor[num]->get_motor_id() | ((uint16_t)mo<<8);
        header.IDE = CAN_ID_EXT;
    } else {
        header.StdId = p_motor[num]->get_motor_id();
        header.IDE = CAN_ID_STD;
    }

    data_len = p_motor[num]->set_motor_dataframe(dataframe, mode, value, len);
    if(data_len > 0) {
        header.DLC = data_len;
        for(uint8_t i = 0; i < data_len; i++) {
            header.Data[i] = dataframe[i];
        }
    } else if(data_len == 0) {
        return 0;
    } else {
        cout << "set_motor_dataframe error!" << endl;
        return -1;
    }
    
    return p_motor[num]->p_can_ctrl->send_func(&header);
}

int Motor_Ctrl::motor_ctrl_init() 
{
    int ret = 0;
    if((nullptr == p_can_ctrl) || (0 == p_can_ctrl->is_ready())) {
        cout << "can is not ready" << endl;
        return -1;
    }
    for(uint8_t i = 0; i < this->num; i++) {
         ret = p_motor[i]->init();
         if(-1 == ret) {
            return -1;
         }
    }
    return ret;
}

int Motor_Ctrl::motor_ctrl_deinit() 
{
    int ret = 0;
    if((nullptr == p_can_ctrl) || (0 == p_can_ctrl->is_ready())) {
        cout << "can is not ready" << endl;
        return -1;
    }
    for(uint8_t i = 0; i < this->num; i++) {
        ret = p_motor[i]->deinit();
        if(-1 == ret) {
            return -1;
        }
    }
    return ret;
}

Motor::~Motor()
{
    std::cout<<"~Motor"<<std::endl;
}

void Motor::set_motor_none()
{
    this->pos = 0;
    this->w   = 0;
    this->t   = 0;
    this->acc = 0;
    this->kp  = 0;
    this->kd  = 0;   
}

int32_t Motor::init()
{
    set_motor_none();
    return 0;
}

int32_t Motor::deinit()
{
    set_motor_none();
    return 0;
}

uint8_t Motor::get_motor_id()
{
    return this->id;
}

Motor_Type Motor::get_motor_type()
{
    return this->type;
}

